#! /usr/bin/env python
import cv2
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
import rospy
from std_msgs.msg import String,Int32MultiArray,Float64MultiArray
from sensor_msgs.msg import Image
import os

class Match:
    def __init__(self):
        self.bridge = CvBridge()
        self.loc = [0,0,1,1]
        self.location_sub = rospy.Subscriber('/camera/loc',Int32MultiArray,self.Callback1)
        self.depth_sub = rospy.Subscriber('/camera/depth/image_raw',Image,self.Callback2)
        self.target_pub = rospy.Publisher('/target/loc',Float64MultiArray,queue_size=10)
        self.std = 0

    def Callback1(self,location):
        try:
            print('getlocation')
            self.loc = location.data
            self.std = 1
        except:
            print('error1')

    def Callback2(self,depth):
        # print('raw',(data.height,data.width))
        cv_image = self.bridge.imgmsg_to_cv2(depth,'32FC1')
        cv2.imshow('depth',cv_image)
        cv2.waitKey(1)
        # print(cv_image.shape)
        if self.std:
            fid=open('./depth.txt', 'wb')
            data = np.array(cv_image[self.loc[1]:self.loc[3],self.loc[0]:self.loc[2]])
            np.savetxt(fid, data, fmt='%.17g', delimiter=' ', newline='\n', header='', footer='', comments='# ')
            fid.close()
            self.std = 0
            x = int((self.loc[0]+self.loc[2])/2)
            y = int((self.loc[1]+self.loc[3])/2)
            print(cv_image[y,x])
            location = [x,y,cv_image[y,x]]
            P = np.matrix([[554.254691191187, 0.0, 320.5, -0.0],[ 0.0, 554.254691191187, 240.5, 0.0],[ 0.0, 0.0, 1.0, 0.0]]) 
            K = np.matrix([[554.254691191187, 0.0, 320.5],[ 0.0, 554.254691191187, 240.5],[ 0.0, 0.0, 1.0]])
            K_ = K.getI()
            u = location[0]*location[2]
            v = location[1]*location[2]
            uvw = np.matrix([u,v,location[2]])
            xyz = K_*(uvw.getT())
            # T = np.array([[0,1,0,-0.087],[0,0,1,0.0125],[1,0,0,0.287],[0,0,0,1]])
            T = np.array([[0,0,1,-0.087],[-1,0,0,0.0125],[0,-1,0,0.287],[0,0,0,1]])
            position = np.matrix([xyz[0,0],xyz[1,0],xyz[2,0],1])
            point = T*position.getT()
            print(point)
            msg = Float64MultiArray(data = point)
            # print(msg)
            self.target_pub.publish(msg)
            # print(data[self.loc[1]:self.loc[3],self.loc[0]:self.loc[2]])

def main():
    rospy.init_node('get_depth',anonymous = False)
    match = Match()
    rospy.spin()

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass
